#include "EEExpUtil.h"
#include "EEExpTree.h"

#include <stdio.h>
#include <math.h>
#include "..\PRide\engine\memory.h"
#include "..\PRide\render\interface_types.h"

//*********************************************
//Vertex
//*********************************************
void vertex::write(FILE* pFile)
{
	if(WRITE_ASCII)
	{
		fprintf(pFile,
		"P(%f,%f,%f) TA(%f,%f)\n", pos.x,pos.y,pos.z,texcoordA.x,texcoordA.y);
	}
	else
	{
		Vertex output;

		EE_ZEROMEMORY(&output, sizeof(Vertex));

		output.position[0] = pos.x;
		output.position[1] = pos.y;
		output.position[2] = pos.z;

		output.texA[0] = texcoordA.x;
		output.texA[1] = texcoordA.y;

		output.texB[0] = texcoordB.x;
		output.texB[1] = texcoordB.y;

		output.texC[0] = texcoordC.x;
		output.texC[1] = texcoordC.y;

		output.normal[0] = normal.x;
		output.normal[1] = normal.y;
		output.normal[2] = normal.z;

		fwrite(&output, sizeof(Vertex), 1, pFile);

		/*fwrite(&pos, sizeof(vector3), 1, pFile);
		fwrite(&texcoord, sizeof(vector2), 1, pFile);*/
	}
}

//*********************************************
//Triangle
//*********************************************
inline vector3 triangle::normal() const
{
	vector3 U = A.pos - B.pos;
	vector3 V = A.pos - C.pos;

	vector3 normal =
			vector3(U.y * V.z - U.z * V.y,
				   U.x * V.z - U.z * V.x,
				   U.x * V.y - U.y * V.x);

	vector3::normalize(normal);
	return normal;
}

bool triangle::split(triangle& tri, plane& splitPlane, triangle& outA, triangle& outB, triangle& outC)
{
	vertex splitA, splitB;

	if(KDTree::isPlaneBetween(splitPlane,tri.A,tri.B))
	{
		if(KDTree::isPlaneBetween(splitPlane,tri.A,tri.C))
		{
			KDTree::vertexAtSplit(splitPlane,tri.A,tri.B,splitA);
			KDTree::vertexAtSplit(splitPlane,tri.A,tri.C,splitB);

			outA.A = tri.A;
			outA.B = splitA;
			outA.C = splitB;

			outB.A = splitB;
			outB.B = tri.B;
			outB.C = tri.C;

			outC.A = splitA;
			outC.B = tri.B;
			outC.C = splitB;
		}
		else if(KDTree::isPlaneBetween(splitPlane,tri.A,tri.B))
		{
			KDTree::vertexAtSplit(splitPlane,tri.A,tri.B,splitA);
			KDTree::vertexAtSplit(splitPlane,tri.C,tri.B,splitB);

			outA.A = tri.B;
			outA.B = splitB;
			outA.C = splitA;

			outB.A = splitA;
			outB.B = splitB;
			outB.C = tri.C;

			outC.A = tri.A;
			outC.B = splitA;
			outC.C = tri.C;
		}
		else
			return false;
	}
	else if(KDTree::isPlaneBetween(splitPlane,tri.A,tri.C) && KDTree::isPlaneBetween(splitPlane,tri.B,tri.C))
	{
		KDTree::vertexAtSplit(splitPlane,tri.A,tri.C,splitA);
		KDTree::vertexAtSplit(splitPlane,tri.B,tri.C,splitB);

		outA.A = tri.A;
		outA.B = tri.B;
		outA.C = splitA;

		outB.A = splitA;
		outB.B = tri.B;
		outB.C = splitB;

		outC.A = splitA;
		outC.B = splitB;
		outC.C = tri.C;
	}
	else
		return false;

	return true;
}

void triangle::write(FILE* pFile)
{
	A.write(pFile);
	B.write(pFile);
	C.write(pFile);
}

//*********************************************
//KDTree Utilities
//*********************************************
int KDTree::depth = 0;
int KDTree::nodecount = 0;

Classify KDTree::classify(plane &classifier, triangle &tri)
{
	const Classify a = classify(classifier, tri.A);
	const Classify b = classify(classifier, tri.B);
	const Classify c = classify(classifier, tri.C);

	int classification = a + b + c;

	//triangle compleltly on left side of plane
	if(classification == 3)
		return Classify::LEFT;

	//triangle compleltly on right side of plane
	if(classification == -3)
		return Classify::RIGHT;

	//a triangle edge lies on the plane
	if(a * b * c == 0)
	{
		if(a + b + c == 0)
			return Classify::INSIDE;

		//we now know that not all a b c will be on an edge (ie prev lines)
		if(a == Classify::INSIDE)
		{
			if(b == Classify::INSIDE || c == b)
				return c;
		}
		if(b == Classify::INSIDE)
		{
			if(c == Classify::INSIDE || c == a)
				return a;
		}
		if(c == Classify::INSIDE)
		{
			if(a == Classify::INSIDE || a == b)
				return b;

		}
	}

	//plane goes through the tirangel
	return Classify::INSIDE;
}

Classify KDTree::classify(plane &classifier, vertex& ver)
{
	return classify(classifier, ver.pos);
}

Classify KDTree::classify(plane &classifier, vector3& vec)
{
	const float classification = vector3::dot(vec,classifier.n) - classifier.d;

	if(classification < 0.0f)
		return Classify::LEFT;

	if(classification > 0.0f)
		return Classify::RIGHT;

	return Classify::INSIDE;
}

bool KDTree::isPlaneBetween(plane& classifier, vertex& A, vertex& B)
{
	const Classify classA = classify(classifier, A);
	const Classify classB = classify(classifier, B);

	if(classA == Classify::INSIDE || classB == Classify::INSIDE)
		return false;

	if(classA != classB)
		return true;

	return false;
}

vertex vertex::baseLerp(float t, vertex& A, vertex& B)
{
	vector2 lerpTexCoord = vector2::baseLerp(t, A.texcoordA, B.texcoordA);
	vector3 lerpPosition = vector3::baseLerp(t, A.pos, B.pos);

	return vertex(lerpPosition, lerpTexCoord);
}

bool KDTree::vertexAtSplit(plane& classifier, vertex& A, vertex& B, vertex& splitPoint)
{
	vector3 segment = B.pos - A.pos;

	if(vector3::dot(segment,classifier.n) != 0.0f)
	{
		const float t = (classifier.d - vector3::dot(A.pos,classifier.n)) / (vector3::dot(segment,classifier.n));

		splitPoint = vertex::baseLerp(t, A,B);
		return true;
	}

	splitPoint = vertex();
	return false;
}

float KDTree::minPositionOnAxis(Axis splitAxis, std::vector<triangle>* triangles)
{
	float minValue = (splitAxis == Axis::X) ? (*triangles)[0].A.pos.x :
					 (splitAxis == Axis::Y) ? (*triangles)[0].A.pos.y :
					 (*triangles)[0].A.pos.z;

	switch(splitAxis)
	{
	case Axis::X:
		{
			for(unsigned int i = 0; i < triangles->size(); i++)
			{
				triangle* tri = &(*triangles)[i];
				if(tri->A.pos.x < minValue)
					minValue = tri->A.pos.x;
				if(tri->B.pos.x < minValue)
					minValue = tri->B.pos.x;
				if(tri->C.pos.x < minValue)
					minValue = tri->C.pos.x;
			}
			break;
		}
	case Axis::Y:
		{
			for(unsigned int i = 0; i < triangles->size(); i++)
			{
				triangle* tri = &(*triangles)[i];
				if(tri->A.pos.y < minValue)
					minValue = tri->A.pos.y;
				if(tri->B.pos.y < minValue)
					minValue = tri->B.pos.y;
				if(tri->C.pos.y < minValue)
					minValue = tri->C.pos.y;
			}
			break;
		}
	case Axis::Z:
		{
			for(unsigned int i = 0; i < triangles->size(); i++)
			{
				triangle* tri = &(*triangles)[i];
				if(tri->A.pos.z < minValue)
					minValue = tri->A.pos.z;
				if(tri->B.pos.z < minValue)
					minValue = tri->B.pos.z;
				if(tri->C.pos.z < minValue)
					minValue = tri->C.pos.z;
			}
			break;
		}
	}

	return minValue;
}

float KDTree::maxPositionOnAxis(Axis splitAxis, std::vector<triangle>* triangles)
{
	float value = (splitAxis == Axis::X) ? (*triangles)[0].A.pos.x :
					(splitAxis == Axis::Y) ? (*triangles)[0].A.pos.y :
					(*triangles)[0].A.pos.z;

	switch(splitAxis)
	{
	case Axis::X:
		for(unsigned int i = 0, j = (unsigned int)triangles->size(); i < j; i++)
		{
			triangle* pTriangle = &((*triangles)[i]);
			if(pTriangle->A.pos.x > value)
				value = pTriangle->A.pos.x;
			if(pTriangle->B.pos.x > value)
				value = pTriangle->B.pos.x;
			if(pTriangle->C.pos.x > value)
				value = pTriangle->C.pos.x;
		}
		break;
	case Axis::Y:
		for(unsigned int i = 0, j = (unsigned int)triangles->size(); i < j; i++)
		{
			triangle* pTriangle = &((*triangles)[i]);
			if(pTriangle->A.pos.y > value)
				value = pTriangle->A.pos.y;
			if(pTriangle->B.pos.y > value)
				value = pTriangle->B.pos.y;
			if(pTriangle->C.pos.y > value)
				value = pTriangle->C.pos.y;
		}
		break;
	case Axis::Z:
		for(unsigned int i = 0, j = (unsigned int)triangles->size(); i < j; i++)
		{
			triangle* pTriangle = &((*triangles)[i]);
			if(pTriangle->A.pos.z > value)
				value = pTriangle->A.pos.z;
			if(pTriangle->B.pos.z > value)
				value = pTriangle->B.pos.z;
			if(pTriangle->C.pos.z > value)
				value = pTriangle->C.pos.z;
		}
		break;
	}
	return value;
}

float maxPositionOnAxis(Axis splitAxis, std::vector<triangle>* triangles)
{
	float value = (splitAxis == Axis::X) ? (*triangles)[0].A.pos.x :
					(splitAxis == Axis::Y) ? (*triangles)[0].A.pos.y :
					(*triangles)[0].A.pos.z;

	switch(splitAxis)
	{
	case Axis::X:
		for(int i = 0, j = (unsigned int)triangles->size(); i < j; i++)
		{
			triangle* pTriangle = &((*triangles)[i]);
			if(pTriangle->A.pos.x > value)
				value = pTriangle->A.pos.x;
			if(pTriangle->B.pos.x > value)
				value = pTriangle->B.pos.x;
			if(pTriangle->C.pos.x > value)
				value = pTriangle->C.pos.x;
		}
		break;
	case Axis::Y:
		for(int i = 0, j = (unsigned int)triangles->size(); i < j; i++)
		{
			triangle* pTriangle = &((*triangles)[i]);
			if(pTriangle->A.pos.y > value)
				value = pTriangle->A.pos.y;
			if(pTriangle->B.pos.y > value)
				value = pTriangle->B.pos.y;
			if(pTriangle->C.pos.y > value)
				value = pTriangle->C.pos.y;
		}
		break;
	case Axis::Z:
		for(int i = 0, j = (unsigned int)triangles->size(); i < j; i++)
		{
			triangle* pTriangle = &((*triangles)[i]);
			if(pTriangle->A.pos.z > value)
				value = pTriangle->A.pos.z;
			if(pTriangle->B.pos.z > value)
				value = pTriangle->B.pos.z;
			if(pTriangle->C.pos.z > value)
				value = pTriangle->C.pos.z;
		}
		break;
	}
	return value;
}

void KDTree::countClassifications(plane& classifier, int* negativeSideCount, int* positiveSideCount, std::vector<triangle>* triangles)
{
	int negativeCount = 0;
	int positiveCount = 0;

	for(unsigned int i = 0, j = (unsigned int)triangles->size(); i < j; i++)
	{
		triangle* tri = &((*triangles)[i]);
		
		if(classify(classifier, tri->A) == Classify::LEFT)
			negativeCount ++;
		else
			positiveCount ++;

		if(classify(classifier, tri->B) == Classify::LEFT)
			negativeCount ++;
		else
			positiveCount ++;

		if(classify(classifier, tri->C) == Classify::LEFT)
			negativeCount ++;
		else
			positiveCount ++;
	}

	*negativeSideCount = negativeCount;
	*positiveSideCount = positiveCount;
}

void KDTree::findOptimalSplitPlane(plane& resultPlane, Axis resultPlaneNormal, std::vector<triangle>* triangles, int itterations)
{
	float startPoint	= minPositionOnAxis(resultPlaneNormal, triangles);
	float endPoint		= maxPositionOnAxis(resultPlaneNormal, triangles);

	float range = endPoint - startPoint;

	int negativeSideCount = 0, positiveSideCount = 0;

	const float d = (startPoint + endPoint) / 2.0f;	//start the test plane in the middle of the two
													//extents of the set being split

	switch(resultPlaneNormal)
	{
		case Axis::X:
			resultPlane = plane(1.0f,0.0f,0.0f,d);
			break;
		case Axis::Y:
			resultPlane = plane(0.0f,1.0f,0.0f,d);
			break;
		case Axis::Z:
			resultPlane = plane(0.0f,0.0f,1.0f,d);
			break;
	}
	
	for(int i = 0; i < itterations; i++)
	{
		KDTree::countClassifications(resultPlane, &negativeSideCount, &positiveSideCount, triangles);

		const int difference = (positiveSideCount - negativeSideCount); //gives us direction about where the optimal split is

		if(difference < 0)
		{
			endPoint = (startPoint + endPoint) / 2.0f;
		}
		else 
		if(difference > 0)
		{
			startPoint = (startPoint + endPoint) / 2.0f;
		}
		else
		{
			break;	//we found a perfect split!
		}

		resultPlane.d = (startPoint + endPoint) / 2.0f;
	}

	resultPlane.d = (startPoint + endPoint) / 2.0f;
}

void KDTree::partionTriangles(plane& parentSplitPlane, std::vector<triangle>* triangles, 
							  Axis parentSplitPlaneAxis, node* parent)
{
	try {

	KDTree::depth ++;
	unsigned int triangleCount = (unsigned int)triangles->size();
	if(KDTree::depth > 50)
		throw "Tree too deep\n";
	if(KDTree::nodecount > 250)
		throw "Too many nodes\n";
	KDTree::nodecount++;

	if(triangles->size() > KDTree::MAX_LEAF_TRIANGLES)
	{
		std::vector<triangle> *leftList = new std::vector<triangle>(), 
							  *rightList = new std::vector<triangle>();
		int leftTriangleCount = 0,
			rightTriangleCount = 0;
		const Axis splitPlaneAxis =	(parentSplitPlaneAxis == Axis::X) ? Axis::Y :
									(parentSplitPlaneAxis == Axis::Y) ? Axis::Z :
									Axis::X;

		plane splitPlane;
		KDTree::findOptimalSplitPlane(splitPlane, splitPlaneAxis, triangles);
	
		int saftey = 0;
		while(triangles->size() > 0)
		{
			triangle curTri = (*triangles)[0];
			triangles->erase(triangles->begin());
			
			if(saftey > 2000)
				throw "Loop wont break\n";
			saftey++;
			
			switch(classify(splitPlane, curTri))
			{
				case Classify::LEFT:
				{
					leftList->push_back(curTri);
					leftTriangleCount++;
					break;
				}
				case Classify::RIGHT:
				{
					rightList->push_back(curTri);
					rightTriangleCount++;
					break;
				}
				case Classify::INSIDE:
				{
					vector3 planeNormal = splitPlane.n; vector3::normalize(planeNormal);
					vector3 trianNormal = curTri.normal();
					
					const float angle = acos(vector3::dot(planeNormal, trianNormal));
					
					if(angle < 0.00872664626f ||/*0.5 degrees*/ 
						angle > 3.13286601f)	/*179.5 degrees*/
					{
						//assume tri is planar with plane so just shuv it in the left list
						leftList->push_back(curTri);
						leftTriangleCount++;
					}
					else
					{
						triangle tri[3];
						if(!triangle::split(curTri,splitPlane,tri[0],tri[1],tri[2]))
							throw -1;

						for(int j = 0; j < 3; j++)
						{
							switch(KDTree::classify(splitPlane, tri[j]))
							{
							case Classify::LEFT:
								leftList->push_back(curTri);
								break;
							case Classify::RIGHT:
								rightList->push_back(curTri);
								break;
							case Classify::INSIDE:
								KDTree::classify(splitPlane, tri[j]);
								throw "Lost triangle from split\n" ;
								break;
							}
						}
					}
				}
			}
		}

		#define TAB for(int i = 0; i < KDTree::depth; i++) std::cout<<" ";

		std::cout<<"----------------------------------"<<std::endl;
		TAB; std::cout<<"Node:\t"<<parent->id<<std::endl;
		TAB;	if(parent->parent) std::cout<<"Parent:\t"<< parent->parent->id<<std::endl;
				else std::cout<<"Parent:\tNULL"<< std::endl;
		TAB; std::cout<<"Depth:\t"<<KDTree::depth<<std::endl;
		TAB; std::cout<<"Triangles:\t"<<triangleCount<<std::endl;
		
		parent->pLeftChild = new node();
		parent->pLeftChild->parent = parent;
		KDTree::treeNodes.push_back(parent->pLeftChild);
		if(leftTriangleCount > KDTree::MAX_LEAF_TRIANGLES && KDTree::depth < KDTree::MAX_TREE_DEPTH)
		{	
			partionTriangles(splitPlane, leftList, splitPlaneAxis,parent->pLeftChild);
		}
		else
		{
			std::cout<<"----------------------------------"<<std::endl;
			TAB; std::cout<<"Leaf:\t"<<parent->pLeftChild->id<<std::endl;
			TAB; std::cout<<"Parent:\t"<<parent->id<<std::endl;
			TAB; std::cout<<"Depth:\t"<<KDTree::depth + 1<<std::endl;
			TAB; std::cout<<"True Triangles:\t"<<leftList->size()<<std::endl;
			TAB; std::cout<<"Cond Triangles:\t"<<leftTriangleCount<<std::endl;

			(parent->pLeftChild)->triangles = leftList;
		}

		parent->pRightChild = new node();
		parent->pRightChild->parent = parent;
		KDTree::treeNodes.push_back(parent->pRightChild);
		if(rightTriangleCount> KDTree::MAX_LEAF_TRIANGLES && KDTree::depth < KDTree::MAX_TREE_DEPTH)
		{	
			partionTriangles(splitPlane, rightList, splitPlaneAxis,parent->pRightChild);
		}
		else
		{
			std::cout<<"----------------------------------"<<std::endl;
			TAB; std::cout<<"Leaf:\t"<<parent->pRightChild->id<<std::endl;
			TAB; std::cout<<"Parent:\t"<<parent->id<<std::endl;
			TAB; std::cout<<"Depth:\t"<<KDTree::depth + 1<<std::endl;
			TAB; std::cout<<"True Triangles:\t"<<rightList->size()<<std::endl;
			TAB; std::cout<<"Cond Triangles:\t"<<rightTriangleCount<<std::endl;

			(parent->pRightChild)->triangles = rightList;
		}

		delete triangles;
		triangles = NULL;
	}
	else
	{
		//Not enough triangles to split
		parent->triangles = triangles;
	}

	}catch(char* error)
	{
		std::cout<<"Error :" << error <<std::endl;
	}

	KDTree::depth --;
}